#include "VideoProcessor.h"

VideoProcessor::VideoProcessor(void)
{
	cam = VideoCapture(0);
	cam.set(CV_CAP_PROP_FRAME_HEIGHT,140);
	cam.set(CV_CAP_PROP_FRAME_WIDTH,160);
	delay=5;
}

VideoProcessor::VideoProcessor(string filename)
{
	
	cam = VideoCapture(filename);
	rate = cam.get(CV_CAP_PROP_FPS);
	delay = 1000/rate;
	
	cam.set(CV_CAP_PROP_FRAME_HEIGHT,140);
	cam.set(CV_CAP_PROP_FRAME_WIDTH,160);
}


VideoProcessor::~VideoProcessor(void)
{
}

void VideoProcessor::run(void)
{
	Scalar color(255,0,0);
	
	Mat I1,I1Surf,I1hsv,V,outRange;
	vector<Mat>hsvChannel;
	while(true)
	{

		cam>>I1;
		imshow("Cam",I1);
		cvtColor(I1,I1hsv,CV_BGR2HSV);
		split(I1hsv,hsvChannel);
		/*imshow("H",hsvChannel[0]);
		imshow("S",hsvChannel[1]);
		imshow("V",hsvChannel[2]);*/
		
		V=hsvChannel[2];

		double min,max;
		
		
		inRange(I1,Scalar(150),Scalar(255),outRange);
		minMaxLoc(outRange,&min,&max);
		cout<<"min: "<<min<<" max:"<<max<<endl;
		imshow("Out",outRange*100);
			//cv::inRange(V,

		waitKey(5);
		//cvInRangeS(
	}

	


	while(false)
	{

		cam>>I1;
	

	cvtColor(I1,I1,CV_BGR2GRAY);

	vector<KeyPoint>keypoints;
	cv::SurfFeatureDetector fd(2500);//soglia
	fd.detect(I1,keypoints);
	drawKeypoints(I1,keypoints,I1Surf,-1,cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
	imshow("Surf",I1Surf);


	//goodFeaturesToTrack
	vector<Point2f> corners;
	int max_corners=100;
	double qualityLevel=.01;
	double minDistance=10.;

	cv::goodFeaturesToTrack(I1,corners,max_corners,qualityLevel,minDistance);
	drawOnImage(I1,corners,cv::Scalar(255,0,0),3,2);

	imshow("output goodFeaturesToTrack",I1);
	waitKey(30);
	}
	
}

vector<Point2f> VideoProcessor::process(void)
{
	
	
	
	vector<Point2f> corners;
	int max_corners=100;
	double qualityLevel=.01;
	double minDistance=10.;
	cout<<"XXX 1"<<endl;
	cv::goodFeaturesToTrack(current,corners,max_corners,qualityLevel,minDistance);
	cout<<"XXX 1.2"<<endl;

	return corners;
}


 // Draw circles at feature point locations on an image
     void VideoProcessor::drawOnImage(cv::Mat &image, const std::vector<cv::Point2f> &points, cv::Scalar color= cv::Scalar(255,255,255), int radius=3, int thickness=2) {
        std::vector<cv::Point2f>::const_iterator it=    points.begin(); 
        // for all corners 
        while (it!=points.end()) {
           // draw a circle at each corner location
           cv::circle(image,*it,radius,color,thickness);
           ++it;
        }
     }

	 void VideoProcessor::detectCircle(cv::Mat &img)
	 {
		 Mat I1 = imread("C:/Users/domenico/Pictures/Fotografo.jpg");
		cvtColor(I1,I1,CV_BGR2GRAY);
		//GaussianBlur(I1,I1,Size(5,5),1.5);
		vector<Vec3f>circles;
		HoughCircles(I1,circles,CV_HOUGH_GRADIENT,
			  2,   // accumulator resolution (size of the image / 2) 
			  50,  // minimum distance between two circles
			  200, // Canny high threshold 
			  100, // minimum number of votes 
			  10, 1000); // min and max radius



		std::vector<cv::Vec3f>::
          const_iterator itc= circles.begin();
	   while (itc!=circles.end()) {
		 cv::circle(I1, 
			cv::Point((*itc)[0], (*itc)[1]), // circle centre
			(*itc)[2],       // circle radius
			cv::Scalar(255), // color 
			2);              // thickness
		 ++itc;   

		 
			}
	   imshow("Result",I1);

	   cout<<circles.size()<<endl;
	   waitKey(0);


	 }


	 Mat VideoProcessor::next(void)
	 {

		Mat out;
		cam>>out;	 
		wait();
		 
		 return out;
	 }

	 void VideoProcessor::colorDetect(int hsv_value,int tolleranza)
	 {
		 cout<<"Soglia: "<<hsv_value<<endl;
		 Mat frame,output,foreground;
		 vector<Mat>piani;
		 BackgroundSubtractorMOG2 bg;
		 vector<vector<Point> > contours;
		 while(true)
		 {
			 cam>>frame; 
			 waitKey(5);

			 medianBlur(frame,frame,5); 
			 bg(frame,foreground,0.01);  

			 findContours(foreground,contours,CV_RETR_LIST,CV_CHAIN_APPROX_NONE);

			 Mat drawing = Mat::zeros( frame.size(), CV_8UC3 );
			  for( int i = 0; i< contours.size(); i++ )
				 {
				   //Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
				   drawContours( drawing, contours, -1, Scalar(0),2);
				 }


			  imshow("drawing",drawing);
			 imshow("frame",frame);
			 imshow("foreground",foreground);

			 //background
			// imshow("background",frame.mul(foreground));

			 cvtColor(frame,frame,CV_BGR2HSV);
			 split(frame,piani);
			 inRange(piani[0],Scalar(hsv_value-tolleranza),Scalar(hsv_value+tolleranza),output);

			 
			  

			 imshow("Output",output);
			 imshow("Dilate",frame);
		 }
	 }

	 void VideoProcessor::wait(void)
	 {
		 waitKey(delay);
	 }